using System;
using System.Collections.Generic;
using System.Text;
using TinyOsMessageForwarder;
using RoverStudio.Common.Controller;
using System.IO;

namespace CaterpillarRover
{
    public class CaterpillarController : ITankController
    {
        #region ITankController Members

        void ITankController.MoveTank(double left, double right, double time)
        {
            MoveTinyOS2(left, right, time);
        }

        #endregion

        private TinyOSMessageForwarder messageForwarder;
        private int address = TinyOS2Message.BROADCAST_ADDRESS;
        
        public int Address {
        	get {
        		return address;
        	}
        	set {
        		address = value;
        	}
        }

        public CaterpillarController(TinyOSMessageForwarder messageForwarder)
        {
            this.messageForwarder = messageForwarder;
            Directory.CreateDirectory(@"d:\debug\messagelog\");
//            logStream = File.AppendText(@"d:\debug\messagelog\log_"+DateTime.Now.Ticks+".txt");
        }

        private int maxSpeed = 32767;
        private StreamWriter logStream;


        public void Move(double leftVoltage, double rightVoltage, double timeToMove)
        {
            CaterpillarControlMessage msg = new CaterpillarControlMessage();
            msg.Address = (ushort)address;
            msg.Group = 0x81;
            leftVoltage = Math.Max(leftVoltage, -1);
            leftVoltage = Math.Min(leftVoltage, 1);
            rightVoltage = Math.Max(rightVoltage, -1);
            rightVoltage = Math.Min(rightVoltage, 1);
            msg.Command = CaterpillarControlMessage.CommandType.MOVE;
            msg.VLeft = (Int16)(leftVoltage * maxSpeed);
            msg.VRight = (Int16)(rightVoltage * maxSpeed);
            msg.Time = (UInt16)(timeToMove * 1024);
            messageForwarder.SendMessage(msg);

        }

        public void MoveTinyOS2(double omegaLeft, double omegaRight, double timeToMove)
        {
            CaterpillarControlMessageTinyOS2 msg = new CaterpillarControlMessageTinyOS2();
            msg.AddressDst = (ushort)address;
            msg.AddressSrc = 0x000C;
            msg.Group = 0x22;
            omegaLeft = Math.Max(omegaLeft, -1);
            omegaLeft = Math.Min(omegaLeft, 1);
            omegaRight = Math.Max(omegaRight, -1);
            omegaRight = Math.Min(omegaRight, 1);
            msg.Mode = (byte)1; // PWM=0, PID=1
            msg.VLeft = (Int16)(omegaLeft * maxSpeed);
            msg.VRight = (Int16)(omegaRight * maxSpeed);
            msg.Time = (UInt16)(timeToMove * 1024);
            messageForwarder.SendMessage(msg);
//            logStream.WriteLine(""+msg.VLeft+" "+msg.VRight+" "+msg.Time+" "+DateTime.Now.Ticks);
//            logStream.Flush();
        }
        /// <summary>
        /// First bit: collision detectors, second bit: position lights.
        /// </summary>
        /// <param name="periferiaNumbers"></param>
        public void PowerPeriferia(UInt16 periferiaNumbers)
        {
            TankPowerMessageTinyOS2 msg = new TankPowerMessageTinyOS2();
            msg.AddressDst = TinyOS2Message.BROADCAST_ADDRESS;
            msg.AddressSrc = 0x000C;
            msg.Group = 0x22;
            msg.Periferia = periferiaNumbers;
            messageForwarder.SendMessage(msg);
        }

        public void CollectECube()
        {
            TowerMessageTinyOS2 msg = new TowerMessageTinyOS2();
            msg.AddressDst = TinyOS2Message.BROADCAST_ADDRESS;
            msg.AddressSrc = 0x000C;
            msg.Group = 0x22;
            msg.TowerCommand = new byte[] { (byte)9};
            messageForwarder.SendMessage(msg);

        }
        
        public void TurnTover(int tenthSecs)
        {
            TowerMessageTinyOS2 msg = new TowerMessageTinyOS2();
            msg.AddressDst = TinyOS2Message.BROADCAST_ADDRESS;
            msg.AddressSrc = 0x000C;
            msg.Group = 0x22;
            unchecked
            {
                msg.TowerCommand = new byte[] { (byte)1, (byte)((sbyte)Math.Sign(tenthSecs) * 100), (byte)Math.Abs(tenthSecs) };
                //msg.TowerCommand = new byte[] { (byte)4, (byte)Math.Abs(tenthSecs) };
            }
            messageForwarder.SendMessage(msg);

        }


    }

    //    typedef struct TankControlMsg {
    //    uint16_t seqnum;  //5
    //    uint8_t cmd;      //7
    //    union {
    //        struct {
    //            int16_t vleft;  //8
    //            int16_t vright; //10
    //            uint16_t time;  //12
    //        } movement;

    //        struct {
    //            uint16_t servo1; //8	
    //            uint16_t servo2; //10	
    //            uint16_t servo3; //12	
    //        } servos;

    //        uint16_t power;	//8

    //    } data;
    //} TankControlMsg;

    public class TowerMessageTinyOS2 : TinyOS2Message
    {
        public TowerMessageTinyOS2()
        {
            AMType = 12;
            Length = 22;

        }
        public UInt16 SeqNum
        {
            get { return ReadUInt16(7); }
            set { WriteUInt16(7, value); }
        }

        //private byte[] towerCommand;

        public byte[] TowerCommand
        {
            get
            {
                byte[] res = new byte[Length-2];
                for (int i = 0; i < Length-2; i++)
                {

                    res[i] = ReadUInt8((byte)(9 + i));
                }
                return res;
            }
            set
            {
                Length = (byte)(value.Length+2);
                for (int i = 0; i < Length-2; i++)
                {
                    WriteUInt8((byte)(9 + i), value[i]);
                }
            }
        }

    }

    public class TankPowerMessageTinyOS2 : TinyOS2Message
    {
        public TankPowerMessageTinyOS2()
        {
            AMType = 11;
            Length = 4;

        }
        public UInt16 SeqNum
        {
            get { return ReadUInt16(7); }
            set { WriteUInt16(7, value); }
        }

        public UInt16 Periferia
        {
            get { return ReadUInt16(9); }
            set { WriteUInt16(9, value); }
        }

    }

    public class CaterpillarControlMessageTinyOS2 : TinyOS2Message
    {
        public CaterpillarControlMessageTinyOS2()
        {
            AMType = 10;
            Length = 9;
        }

        public UInt16 SeqNum
        {
            get { return ReadUInt16(7); }
            set { WriteUInt16(7, value); }
        }

        public byte Mode
        {
            get { return ReadUInt8(9); }
            set
            {
                WriteUInt8(9, value);
            }
        }
        public Int16 VLeft
        {
            get { return (Int16)ReadUInt16(10); }
            set { WriteUInt16(10, (UInt16)value); }
        }
        public Int16 VRight
        {
            get { return (Int16)ReadUInt16(12); }
            set { WriteUInt16(12, (UInt16)value); }
        }
        public UInt16 Time
        {
            get { return ReadUInt16(14); }
            set { WriteUInt16(14, value); }
        }
    }

    public class CaterpillarControlMessage : TinyOSMessage
    {
        public enum CommandType { MOVE = 0, SETSERVOS = 1, SETPOWER = 2 };
        public CaterpillarControlMessage()
        {
            AMType = 10;
        }

        public UInt16 SeqNum
        {
            get { return ReadUInt16(5); }
            set { WriteUInt16(5, value); }
        }

        public CommandType Command
        {
            get { return (CommandType)ReadUInt8(7); }
            set
            {
                WriteUInt8(7, (byte)value);
                switch (value)
                {
                    case CommandType.MOVE:
                        Length = 14;
                        break;
                    case CommandType.SETSERVOS:
                        Length = 14;
                        break;
                    case CommandType.SETPOWER:
                        Length = 10;
                        break;
                }
            }
        }
        public Int16 VLeft
        {
            get { return (Int16)ReadUInt16(8); }
            set { WriteUInt16(8, (UInt16)value); }
        }
        public Int16 VRight
        {
            get { return (Int16)ReadUInt16(10); }
            set { WriteUInt16(10, (UInt16)value); }
        }
        public UInt16 Time
        {
            get { return ReadUInt16(12); }
            set { WriteUInt16(12, value); }
        }

    }

    
    public class TankMoveMessageTinyOS2 : TinyOS2Message
    {
        public TankMoveMessageTinyOS2()
        {
            AMType = 10;
            Length = 9;
        }

        public UInt16 SeqNum
        {
            get { return ReadUInt16(7); }
            set { WriteUInt16(7, value); }
        }

        public byte Mode
        {
            get { return ReadUInt8(9); }
            set
            {
                WriteUInt8(9, value);
            }
        }
        public Int16 VLeft
        {
            get { return (Int16)ReadUInt16(10); }
            set { WriteUInt16(10, (UInt16)value); }
        }
        public Int16 VRight
        {
            get { return (Int16)ReadUInt16(12); }
            set { WriteUInt16(12, (UInt16)value); }
        }
        public UInt16 Time
        {
            get { return ReadUInt16(14); }
            set { WriteUInt16(14, value); }
        }
    }
    
}
